<%
if !defined?(name) || name== nil || name.empty?
  $model_name = 'usv'
else
  $model_name = name
end

$arm_name = nil
if defined?(arm)
  $arm_name = arm
end

if !defined?(gripper) || gripper== nil || gripper.empty?
  $gripper_name = 'mbzirc_oberon7_gripper'
else
  $model_name = gripper
end

if !defined?(wavefieldSize) || wavefieldSize == nil || wavefieldSize.empty?
  $wavefield_size = 1000.0
else
  $wavefield_size = wavefieldSize.to_f
end
  $wavefield_cell_count = $wavefield_size / 20.0

  x_uu = 72.4 # Hydrodynamic quadratic coefficient
  x_u = 51.3 # Hydrodynamic linear coefficient
  max_velocity_knots = 8 # Maximum velocity in knots. Taken from the USV specification.
  max_velocity_mps = max_velocity_knots * 0.5144 # convert knots to m/s
  max_total_thrust = (x_u + x_uu * max_velocity_mps) * max_velocity_mps

$slot0_payload = nil
$slot0_rpy = '0 0 0'

$slot1_payload = nil
$slot1_rpy = '0 0 0'

$slot2_payload = nil
$slot2_rpy = '0 0 0'

$slot3_payload = nil
$slot3_rpy = '0 0 0'

$arm_slot_payload = '0'
if defined?(arm_slot)
  $arm_slot_payload = arm_slot
end
if defined?(slot0)
  $slot0_payload = slot0
end
if defined?(slot0_pos)
  $slot0_rpy = slot0_pos
end
if defined?(slot1)
  $slot1_payload = slot1
end
if defined?(slot1_pos)
  $slot1_rpy = slot1_pos
end
if defined?(slot2)
  $slot2_payload = slot2
end
if defined?(slot2_pos)
  $slot2_rpy = slot2_pos
end
if defined?(slot3)
  $slot3_payload = slot3
end
if defined?(slot3_pos)
  $slot3_rpy = slot3_pos
end
%>

<?xml version="1.0"?>

<sdf version='1.9'>
<model name="<%= $model_name%>">
  <static>false</static>
  <self_collide>true</self_collide>

  <!-- Platform base model-->
  <include merge="true">
    <uri>model://mbzirc_usv_base</uri>
  </include>

  <% if $arm_name != nil%>
  <include>
    <name>arm</name>
    <uri>model://<%= $arm_name%></uri>
    <placement_frame>arm_mount_point</placement_frame>
    <pose relative_to="arm_slot_<%= $arm_slot_payload%>">0 0 0 0 0 0</pose>
  </include>
  <joint name="arm_slot_joint" type="fixed">
    <parent>arm_slot_<%= $arm_slot_payload%></parent>
    <child>arm::arm_mount_point</child>
  </joint>
  <% end %>

  <% if $slot0_payload != nil%>
  <!-- Payload slot0 -->
  <include>
    <name>sensor_0</name>
    <uri>model://sensors/<%= $slot0_payload%></uri>
    <placement_frame>mount_point</placement_frame>
    <pose relative_to="slot_0" degrees="true">
      0 0 0 <%= $slot0_rpy%>
    </pose>
  </include>

  <joint name="slot_0_joint" type="fixed">
    <parent>slot_0</parent>
    <child>sensor_0::mount_point</child>
  </joint>
  <% end %>
  <% if $slot1_payload != nil%>
  <!-- Payload slot1 -->
  <include>
    <name>sensor_1</name>
    <uri>model://sensors/<%= $slot1_payload%></uri>
    <placement_frame>mount_point</placement_frame>
    <pose relative_to="slot_1" degrees="true">
      0 0 0 <%= $slot1_rpy%>
    </pose>
  </include>

  <joint name="slot_1_joint" type="fixed">
    <parent>slot_1</parent>
    <child>sensor_1::mount_point</child>
  </joint>
  <% end %>
  <% if $slot2_payload != nil%>
  <!-- Payload slot2 -->
  <include>
    <name>sensor_2</name>
    <uri>model://sensors/<%= $slot2_payload%></uri>
    <placement_frame>mount_point</placement_frame>
    <pose relative_to="slot_2" degrees="true">
      0 0 0 <%= $slot2_rpy%>
    </pose>
  </include>

  <joint name="slot_2_joint" type="fixed">
    <parent>slot_2</parent>
    <child>sensor_2::mount_point</child>
  </joint>
  <% end %>
  <% if $slot3_payload != nil%>
  <!-- Payload slot3 -->
  <include>
    <name>sensor_3</name>
    <uri>model://sensors/<%= $slot3_payload%></uri>
    <placement_frame>mount_point</placement_frame>
    <pose relative_to="slot_3" degrees="true">
      0 0 0 <%= $slot3_rpy%>
    </pose>
  </include>

  <joint name="slot_3_joint" type="fixed">
    <parent>slot_3</parent>
    <child>sensor_3::mount_point</child>
  </joint>
  <% end %>
  <% if $slot4_payload != nil%>
  <!-- Payload slot4 -->
  <include>
    <name>sensor_4</name>
    <uri>model://sensors/<%= $slot4_payload%></uri>
    <placement_frame>mount_point</placement_frame>
    <pose relative_to="slot_4" degrees="true">
      0 0 0 <%= $slot4_rpy%>
    </pose>
  </include>

  <joint name="slot_4_joint" type="fixed">
    <parent>slot_4</parent>
    <child>sensor_4::mount_point</child>
  </joint>
  <% end %>

  <plugin
    filename="ignition-gazebo-thruster-system"
    name="ignition::gazebo::systems::Thruster">
    <joint_name>left_engine_propeller_joint</joint_name>
    <thrust_coefficient>0.004422</thrust_coefficient>
    <fluid_density>1000</fluid_density>
    <propeller_diameter>0.2</propeller_diameter>
    <velocity_control>true</velocity_control>
    <max_thrust_cmd><%= max_total_thrust/2 %></max_thrust_cmd>
  </plugin>

  <plugin
    filename="ignition-gazebo-joint-position-controller-system"
    name="ignition::gazebo::systems::JointPositionController">
    <joint_name>left_chassis_engine_joint</joint_name>
    <use_velocity_commands>true</use_velocity_commands>
    <topic><%= $model_name%>/left/thruster/joint/cmd_pos</topic>
  </plugin>

  <plugin
    filename="ignition-gazebo-thruster-system"
    name="ignition::gazebo::systems::Thruster">
    <joint_name>right_engine_propeller_joint</joint_name>
    <thrust_coefficient>0.004422</thrust_coefficient>
    <fluid_density>1000</fluid_density>
    <propeller_diameter>0.2</propeller_diameter>
    <velocity_control>true</velocity_control>
    <max_thrust_cmd><%= max_total_thrust/2 %></max_thrust_cmd>
  </plugin>

  <plugin
    filename="ignition-gazebo-joint-position-controller-system"
    name="ignition::gazebo::systems::JointPositionController">
    <joint_name>right_chassis_engine_joint</joint_name>
    <use_velocity_commands>true</use_velocity_commands>
    <topic><%= $model_name%>/right/thruster/joint/cmd_pos</topic>
  </plugin>

  <plugin
    filename="libSurface.so"
    name="ignition::gazebo::systems::Surface">
    <link_name>base_link</link_name>
    <vehicle_length>6</vehicle_length>
    <vehicle_width>3.3</vehicle_width>
    <hull_radius>0.27</hull_radius>
    <fluid_level>0.45</fluid_level>

    <!-- Waves -->
    <wavefield>
      <size><%= $wavefield_size%> <%= $wavefield_size%></size>
      <cell_count><%= $wavefield_cell_count%> <%=$wavefield_cell_count%></cell_count>
      <wave>
        <model>PMS</model>
        <period>5</period>
        <number>3</number>
        <scale>1.1</scale>
        <gain>0.3</gain>
        <direction>1 0</direction>
        <angle>0.4</angle>
        <tau>2.0</tau>
        <amplitude>0.0</amplitude>
        <steepness>0.0</steepness>
      </wave>
    </wavefield>
  </plugin>

  <plugin
    filename="libSimpleHydrodynamics.so"
    name="ignition::gazebo::systems::SimpleHydrodynamics">
    <link_name>base_link</link_name>
    <!-- Added mass -->
    <xDotU>0.0</xDotU>
    <yDotV>0.0</yDotV>
    <nDotR>0.0</nDotR>
    <!-- Linear and quadratic drag -->
    <xU><%=x_u%></xU>
    <xUU><%=x_uu%></xUU>
    <yV>40.0</yV>
    <yVV>0.0</yVV>
    <zW>500.0</zW>
    <kP>100.0</kP>
    <mQ>100.0</mQ>
    <nR>400.0</nR>
    <nRR>0.0</nRR>
  </plugin>

  <!-- Publish robot state information -->
  <plugin filename="libignition-gazebo-pose-publisher-system.so"
    name="ignition::gazebo::systems::PosePublisher">
    <publish_link_pose>true</publish_link_pose>
    <publish_sensor_pose>true</publish_sensor_pose>
    <publish_collision_pose>false</publish_collision_pose>
    <publish_visual_pose>false</publish_visual_pose>
    <publish_nested_model_pose>true</publish_nested_model_pose>
    <publish_model_pose>false</publish_model_pose>
    <use_pose_vector_msg>true</use_pose_vector_msg>
    <static_publisher>true</static_publisher>
    <static_update_frequency>1</static_update_frequency>
  </plugin>

  <!-- These plugins detect when target objects are placed at the
       the defined region. A message is published on the <topic> when these
       events occur -->
  <plugin filename="libEntityDetector.so"
          name="mbzirc::EntityDetector">
    <topic>/mbzirc/target_object_detector/placed</topic>
    <pose>0 0 0.22 0 0 0</pose>
    <geometry>
      <box>
        <size>5 3.3 0.25</size>
      </box>
    </geometry>
  </plugin>
  <plugin filename="libEntityDetector.so"
          name="mbzirc::EntityDetector">
    <topic>/mbzirc/target_object_detector/placed</topic>
    <pose>0 0 0.8 0 0 0</pose>
    <geometry>
      <box>
        <size>1.5 3.5 0.25</size>
      </box>
    </geometry>
  </plugin>
  <plugin filename="libEntityDetector.so"
          name="mbzirc::EntityDetector">
    <topic>/mbzirc/target_object_detector/placed</topic>
    <pose>0 -1.35 0.22 0 0 0</pose>
    <geometry>
      <box>
        <size>6.0 0.58 0.25</size>
      </box>
    </geometry>
  </plugin>
  <plugin filename="libEntityDetector.so"
          name="mbzirc::EntityDetector">
    <topic>/mbzirc/target_object_detector/placed</topic>
    <pose>0 1.35 0.22 0 0 0</pose>
    <geometry>
      <box>
        <size>6.0 0.58 0.25</size>
      </box>
    </geometry>
  </plugin>
  <!-- Comms endpoint-->
  <plugin
    filename="ignition-gazebo-comms-endpoint-system"
    name="ignition::gazebo::systems::CommsEndpoint">
    <address><%= $model_name%></address>
    <topic>model/<%= $model_name%>/rx</topic>
  </plugin>

  <!-- uncomment the following to visualize valid region for placement
      of target objects -->
  <!--
  <link name='target_placement_region'>
    <visual name="bottom_platform">
      <pose>0 0 0.22 0 0 0</pose>
      <transparency>0.5</transparency>
      <geometry>
        <box>
          <size>5 3.3 0.25</size>
        </box>
      </geometry>
      <material>
        <ambient>0.0 1.0 0.0 0.5</ambient>
        <diffuse>0.0 1.0 0.0 0.5</diffuse>
        <specular>0.5 0.5 0.5 0.5</specular>
      </material>
    </visual>
    <visual name="top_platform">
      <pose>0 0 0.8 0 0 0</pose>
      <transparency>0.5</transparency>
      <geometry>
        <box>
          <size>1.5 3.5 0.25</size>
        </box>
      </geometry>
      <material>
        <ambient>0.0 1.0 0.0 0.5</ambient>
        <diffuse>0.0 1.0 0.0 0.5</diffuse>
        <specular>0.5 0.5 0.5 0.5</specular>
      </material>
    </visual>
    <visual name="left_hull">
      <pose>0 -1.35 0.22 0 0 0</pose>
      <transparency>0.5</transparency>
      <geometry>
        <box>
          <size>6.0 0.58 0.25</size>
        </box>
      </geometry>
      <material>
        <ambient>0.0 1.0 0.0 0.5</ambient>
        <diffuse>0.0 1.0 0.0 0.5</diffuse>
        <specular>0.5 0.5 0.5 0.5</specular>
      </material>
    </visual>
    <visual name="right_hull">
      <pose>0 1.35 0.22 0 0 0</pose>
      <transparency>0.5</transparency>
      <geometry>
        <box>
          <size>6.0 0.58 0.25</size>
        </box>
      </geometry>
      <material>
        <ambient>0.0 1.0 0.0 0.5</ambient>
        <diffuse>0.0 1.0 0.0 0.5</diffuse>
        <specular>0.5 0.5 0.5 0.5</specular>
      </material>
    </visual>
    <cast_shadows>false</cast_shadows>
  </link>
  <joint name="target_placement_joint" type="fixed">
    <parent>target_placement_region</parent>
    <child>base_link</child>
  </joint>
  -->

</model>
</sdf>
